/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package org.riverhillrobotics.frc2013.IntegratedCode2013;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class IntegratedCode2013 extends IterativeRobot {
    Joystick leftStick,rightStick,turretStick;
    Joystick xboxShooter,xboxDriver;
    RobotDrive drive;
    Jaguar shooterClose, shooterFar,turret;
    Relay anglerLeft,anglerRight;
    Relay feeder;
    Victor climber;
    
    AxisCamera camera;
    
    DigitalInput anglerSwitch; //Limit switch that stops the shooter from coming down to far
    boolean debounce = false;
    boolean debounce2 = false;
    double shooterSpeed;
    
    boolean doAim = false;
    
    // Initialize the VisionTarget class
    VisionTarget vision;
    
    // Initialize the VisionAim class
    VisionAim aim;
    
    // Initialize the FrisbeeShooter class
    FrisbeeShooter frisbeeShooter;
    
    public void robotInit() 
    {
        leftStick = new Joystick(1); //Port 1
        rightStick = new Joystick(2); //Port 2
        xboxShooter = new Joystick(3); //Port 3
        xboxDriver = new Joystick(4);
        drive = new RobotDrive(1,2,3,4); //Initializes the Robot Drive
        shooterClose = new Jaguar(5);
        shooterFar = new Jaguar(6);
        turret = new Jaguar(7);
        feeder = new Relay(7);//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!
        anglerLeft = new Relay(1);//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!
        anglerRight = new Relay(8);//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!//RELAY INPUT 5 NOT FUNCTIONAL!
        climber = new Victor(8);
        
        camera = AxisCamera.getInstance();
        camera.writeRotation(AxisCamera.RotationT.k180);
        
        anglerSwitch = new DigitalInput(1);
        shooterSpeed=1.0;
        
        // Set the threshold for light green
        //HSVThreshold thres = new HSVThreshold(103, 124, 225, 255, 234, 255, true);
        // GREEN:
        HSVThreshold thres = new HSVThreshold(86, 119, 61, 255, 131, 255, true);
        // BLUE:
        // HSVThreshold thres = new HSVThreshold(157, 176, 114, 255, 110, 255, true);
        vision = new VisionTarget(camera, thres);
        // OR using a custom threshold:
        // HSVThreshold thres = new HSVThreshold(0, 0, 60, 0, 0, 100);
        // vision = new VisionTarget(camera, thres);
        // Can be updated with vision.changeThreshold(newHSVThresholdObj)
        
        aim = new VisionAim(vision, turret, anglerLeft, anglerRight);
        // OR specifying a x and y offset:
        // aim = new VisionAim(vision, turretMotor, shooterElevatorMotor1, shooterElevatorMotor2, -0.5, 0.1);
        
        frisbeeShooter = new FrisbeeShooter(aim, shooterClose, shooterFar, feeder);
        
        SmartDashboard.putString("Robot program", this.getClass().getName());
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() 
    {
        frisbeeShooter.shoot();
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() 
    {
        if(leftStick.getTrigger())
        {
            drive.tankDrive(leftStick.getY(), leftStick.getY());
        }
        else if(rightStick.getTrigger())
        {
                //arcadeDrive//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
        }else
        {
            drive.tankDrive(leftStick.getY(), rightStick.getY());
        }
        //shooting///////////////////////////////////////////////////////////////////////////////////
        // 16 presses MAX
        
        /*if(xboxShooter.getRawButton(1))
        {
            shooterSpeed=.25;
        }*/
        if(xboxShooter.getRawButton(2))
        {
            shooterSpeed=.5;
        }
        if(xboxShooter.getRawButton(3))
        {
            shooterSpeed=.75;
        }
        if(xboxShooter.getRawButton(4))
        {
            shooterSpeed=1.0;
        }
        if(xboxShooter.getRawButton(5))
        {
            shooterClose.set(-shooterSpeed);
            shooterFar.set(-shooterSpeed);
        }/* else if (xboxShooter.getRawButton(7)){
            shooterClose.set(.1);
            shooterFar.set(.1);
        }*/
        else
        {
            shooterClose.set(0.0);
            shooterFar.set(0.0); 
        }
       if( (xboxShooter.getRawButton(5) || xboxShooter.getRawButton(7)) && xboxShooter.getRawButton(6)) //(Button 5 or Button 7) and Button 6
        //if(  xboxShooter.getRawButton(6)) //(Button 5 or Button 7) and Button 6
        {
            feeder.set(Relay.Value.kForward);
        } else if(xboxShooter.getRawButton(8)) {
             feeder.set(Relay.Value.kReverse);
        }else{
            feeder.set(Relay.Value.kOff);
        }
       
       if(xboxShooter.getRawButton(11))
            {
           climber.set(.5);
           Timer.delay(.1);
           climber.set(0);
            }
       else if(xboxShooter.getRawButton(12))
               {
                climber.set(-.3);
                Timer.delay(.1);
                climber.set(0);
               }
       else
       {
           climber.set(0);
       }
        
        if(Math.abs(xboxShooter.getRawAxis(4))>.5)
        {
            if(xboxShooter.getRawAxis(4)>.0)
            {
                anglerLeft.set(Relay.Value.kForward);
                anglerRight.set(Relay.Value.kForward);
            }else{
                anglerLeft.set(Relay.Value.kReverse);
                anglerRight.set(Relay.Value.kReverse);
            }
        }else{
                anglerLeft.set(Relay.Value.kOff);
                anglerRight.set(Relay.Value.kOff);
        }
        if(Math.abs(xboxShooter.getY())>.5){
            if(xboxShooter.getY()>.5)
            {
                anglerLeft.set(Relay.Value.kForward);
            }else{
                anglerLeft.set(Relay.Value.kReverse);
            }
        }
        if(Math.abs(xboxShooter.getZ()) > 0.05)
            turret.set((.5)*(xboxShooter.getZ()));
        
        if(xboxShooter.getRawButton(1))
            if (doAim)
                doAim = false;
            else
                doAim = true;
        if (doAim)
            if (!aim.aimForTarget())
                doAim = false;
        SmartDashboard.putBoolean("Aiming", doAim);
        ////////////////////////////////////////////////////////////SMARTDASHBOARD//////////////////////////////////////////
        
        SmartDashboard.putNumber("Turret Speed:", turret.getSpeed());
        SmartDashboard.putNumber("Shooter Speed (Close):", -shooterClose.getSpeed());
        SmartDashboard.putNumber("Shooter Speed (Far):", -shooterFar.getSpeed());
    }
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() 
    {
        aim.aimForTarget();
    }
    
}
